#include "FIFO.h"
#include "FreeRTOS.h"
#include "sys.h"

// **************************************************************
// Function: FIFO_Init
// Parameters: FIFO type pointer address, queue size
// Return: > = 0 initialization success
// Description: Initialize the FIFO queue
// **************************************************************

u32 FIFO_Init(FIFOTYPE **fifo, u32 fifosize)
{
	if (fifo == NULL || fifosize == 0)
	{
		return FIFO_ERROR_PARAM;
	}

#ifdef configSUPPORT_DYNAMIC_ALLOCATION
	*fifo = pvPortMalloc(sizeof(FIFOTYPE));
#else
	*fifo = malloc(sizeof(FIFOTYPE));
#endif
	if ((*fifo) == NULL)
	{
		return FIFO_ERROR_MEM;
	}

#ifdef configSUPPORT_DYNAMIC_ALLOCATION
	(*fifo)->buffer = pvPortMalloc(fifosize);
#else
	(*fifo)->buffer = malloc(fifosize);
#endif
	if ((*fifo)->buffer == NULL)
	{
		return FIFO_ERROR_MEM;
	}

	(*fifo)->size = fifosize;
	(*fifo)->staraddr = (u32)(&(*fifo)->buffer[0]);                       //Record FIFO buffer start address
	(*fifo)->endaddr = (u32)(&(*fifo)->buffer[fifosize - 1]);               //Record FIFO buffer end address
	(*fifo)->front = (*fifo)->staraddr;                                   //FIFO next read data address

	(*fifo)->CNDTR = fifosize;

	return FIFO_OK;
}

// **************************************************************
// Function: FIFO_Clear
// Parameter: None
// Return: None
// Description: empty FIFO queue
// **************************************************************
u32 FIFO_Clear(FIFOTYPE *fifo)
{
    volatile u32 da;
    if (fifo == NULL)
        return FIFO_ERROR_PARAM;
    fifo->front = fifo->staraddr; //Set the next read address to start with FIFO buffer
	fifo->CNDTR = fifo->size;

    return FIFO_OK;
}

// **************************************************************
// Function: FIFO_Remove
// Parameter: None
// Return: None
// Description: remove the FIFO queue part of the data
// **************************************************************
u32 FIFO_Remove(FIFOTYPE *fifo, u32 RemoveNum)
{
    if (fifo == NULL)
        return FIFO_ERROR_PARAM;

    for (int i = 0; i < RemoveNum; i++)
    {
        if (fifo->front == fifo->endaddr)
        {
            fifo->front = fifo->staraddr;
        }
        else
        {
            fifo->front++; //Set the next read address to start with FIFO buffer
        }
    }

    return FIFO_OK;
}

// **************************************************************
// Function: FIFO_Read
// Parameters: queue pointer, 1byte data pointer
// Return: > = 0 read successfully
// Description: read 1 byte data from the FIFO queue
// **************************************************************
u32 FIFO_Read(FIFOTYPE *fifo, u8 *data, u8 mode, u32 cndtr)
{
	if (fifo == NULL)
        return FIFO_ERROR_PARAM;
    if (FIFO_Status(fifo, cndtr) == 0)
    {
        return FIFO_ERROR_EMPTY;
    }
    *data = (u8)(*((u8 *)(fifo->front)));
    if (fifo->front == fifo->endaddr)
    {
        if (mode)
            fifo->front = fifo->staraddr;
    }
    else
    {
        if (mode)
            fifo->front++;
    }
    return FIFO_OK;
} 

// **************************************************************
// Function: FIFO_ReadN
// Parameters: queue pointer, 1byte data pointer
// Return: = 0 read successfully
// Description: read N byte data from the FIFO queue
// **************************************************************
u32 FIFO_ReadN(FIFOTYPE *fifo, u8 *data, u16 length, u32 cndtr)
{
    if (fifo == NULL)
        return FIFO_ERROR_PARAM;
    if (FIFO_Status(fifo, cndtr) < length)
    {
        return FIFO_ERROR_EMPTY;
    }

    for (int i = 0; i < length; i++)
    {
        *data++ = (u8)(*((u8 *)(fifo->front)));
        if (fifo->front == fifo->endaddr)
        {
            fifo->front = fifo->staraddr;
        }
        else
        {
            fifo->front++; //Set the next read address to start with FIFO buffer
        }
    }
    return FIFO_OK;
}

// **************************************************************
// Function: FIFO_Status
// Parameters: queue pointer
// Return: > 0 queue has not read data
// Description: Get FIFO queue status
// **************************************************************
u32 FIFO_Status(FIFOTYPE *fifo, u32 cndtr)
{
    int res;
    int nextsave;
	
	nextsave = (u32)fifo->endaddr + 1 - cndtr;
	
    res = nextsave - (u32)(fifo->front);
    if (res < 0)
    {
        res = ((u32)(fifo->endaddr) + 1 - (u32)(fifo->front)) + (nextsave - (u32)fifo->staraddr);
    }
    return res;
}

/**
 * @description: 
 * @param {FIFOTYPE} *fifo
 * @param {u8} *data
 * @param {u16} length
 * @param {u32} cndtr
 * @return {*}
 */
u32 FIFO_tryReadN(FIFOTYPE *fifo, u8 *data, u16 length, u32 cndtr)
{
    if (fifo == NULL)
        return FIFO_ERROR_PARAM;
    if (FIFO_Status(fifo, cndtr) < length)
    {
        return FIFO_ERROR_EMPTY;
    }
    u32 front = fifo->front;
    for (int i = 0; i < length; i++)
    {
        *data++ = (u8)(*((u8 *)(front)));
        if (front == fifo->endaddr)
        {
            front = fifo->staraddr;
        }
        else
        {
            front++; //Set the next read address to start with FIFO buffer
        }
    }
    return FIFO_OK;
}
